import math

from Point import *
from environment import *

SMOOTH = 1.0
env = None
def strategy(e):
    global env
    env = e
    e.homeRobots[0].vLeft = 125
    e.homeRobots[0].vRight = darVeldeGiro(3)    
    
    bot = e.homeRobots[1]
    pursuitBall(bot)

    bot = e.homeRobots[2]
    goToPoint(bot, Point([20,30]),True)

    bot = e.homeRobots[3]
    pursuitBall(bot)
    
    bot = e.homeRobots[4]
    pursuitBall(bot)

	
def darVeldeGiro(radio, velocity=125):
    vel = velocity * ((radio - (2.9527/2))/(radio + (2.9527/2)))
    return vel

def pursuitBall(bot):
    goToPoint(bot, env.currentBall)
            
def goToPoint(bot, point, stopWhenArrive = False):
    global SMOOTH
    ang = point.angle(bot.pos)    
    dr = ang - bot.rotation    
    dr = normalizarAngulo(dr)    
   
    if stopWhenArrive:
        dist = point.dist(bot.pos)
        if dist < 1.5:
            bot.vLeft = 0;
            bot.vRight = 0;
            return    
    
    if abs(dr) < 0.0:
        bot.vLeft = 125;
        bot.vRight = 125;
    else:
        if dr < 0:
            bot.vLeft = 125 * SMOOTH
            bot.vRight = (125 + dr) * SMOOTH
        else:
            bot.vLeft = (125 - dr) * SMOOTH
            bot.vRight = 125 * SMOOTH
            
    if (dr > 90) or (dr < -90):       
        if dr < 0:
            dr = dr + 180
            bot.vLeftt = (125 + dr) * SMOOTH * -1
            bot.vRight = 125 * SMOOTH * -1
        else:
            dr = dr - 180
            bot.vLeft = 125 * SMOOTH * -1   
            bot.vRight = (125 - dr) * SMOOTH * -1
            
def normalizarAngulo(ang):
    normal = ang
    if ang > 180:
        normal = ang - 360
    if ang < -180:
        normal = ang + 360        
    return normal


  
